#include "math.h"

//////////////////////////////////////////////////////////////////////////
void ComputeQuaternionW(D3DXQUATERNION& q)
{
	float wSquare = 1.0f - q.x*q.x - q.y*q.y - q.z*q.z;
	if(wSquare<0){
		q.w = 0.0f;
	}
	else{
		q.w = sqrt(wSquare);
	}
}

void QuatInvs(D3DXQUATERNION& q,D3DXQUATERNION& qInvs)
{
	qInvs.x = -q.x;
	qInvs.y = -q.y;
	qInvs.z = -q.z;
	qInvs.w =  q.w;
}


void QuatNormalize(D3DXQUATERNION& q,D3DXQUATERNION& qOut)
{
	float magnitude = sqrt( q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
	if(magnitude > 0.0f)
	{
		float invsMag = 1.0f / magnitude;
		qOut.x = q.x * invsMag;
		qOut.y = q.y * invsMag;
		qOut.z = q.z * invsMag;
		qOut.w = q.w * invsMag;
	}
}

void QuatMulQuat(D3DXQUATERNION& q1,D3DXQUATERNION& q2,D3DXQUATERNION& out)
{
	out.w = (q1.w * q2.w) - (q1.x * q2.x) - (q1.y * q2.y) - (q1.z * q2.z);
	out.x = (q1.x * q2.w) + (q1.w * q2.x) + (q1.y * q2.z) - (q1.z * q2.y);
	out.y = (q1.y * q2.w) + (q1.w * q2.y) + (q1.z * q2.x) - (q1.x * q2.z);
	out.z = (q1.z * q2.w) + (q1.w * q2.z) + (q1.x * q2.y) - (q1.y * q2.x);
}
void QuatMulVec(D3DXQUATERNION& q,D3DXVECTOR3& v,D3DXQUATERNION& out)
{
	out.w = -( q.x * v.x ) - ( q.y * v.y ) - ( q.z * v.z );
	out.x =  ( q.w * v.x ) + ( q.y * v.z ) - ( q.z * v.y );
	out.y =  ( q.w * v.y ) + ( q.z * v.x ) - ( q.x * v.z );
	out.z =  ( q.w * v.z ) + ( q.x * v.y ) - ( q.y * v.x );
}
void QuatLinearInterp( D3DXQUATERNION& q1, D3DXQUATERNION& q2, float t, D3DXQUATERNION& out )
{
	QuatNormalize( q1, q1 );
	QuatNormalize( q2, q2 );
	out = ( 1 - t ) * q1 + t * q2;
	QuatNormalize( out,out );
}


void QuatRotatePoint(D3DXQUATERNION& q,D3DXVECTOR3& v,D3DXVECTOR3& out)
{
	D3DXQUATERNION quatInvs,quatTemp,quatFinal;
	QuatInvs( q, quatInvs );
	QuatNormalize( quatInvs, quatInvs);
	
	QuatMulVec( quatInvs, v, quatTemp );
	QuatMulQuat( quatTemp, q, quatFinal );

	out.x = quatFinal.x;
	out.y = quatFinal.y;
	out.z = quatFinal.z;
}

void QuatRotatePointMat ( D3DXQUATERNION& q, D3DXVECTOR3&v, D3DXVECTOR3& out )
{
	D3DXMATRIX mat;
	QuatToMatrix(q,mat);
	D3DXVECTOR4 temp;
	D3DXVec3Transform(&temp,&v,&mat);

	out.x = temp.x;
	out.y = temp.y;
	out.z = temp.z;
}

void QuatToMatrix(D3DXQUATERNION& q,D3DXMATRIX& mat)
{
	D3DXMATRIX matQuaternion (
		1 - 2 * q.y * q.y - 2 * q.z * q.z, 2 * q.x * q.y - 2 * q.w * q.z, 2 * q.x * q.z + 2 * q.w * q.y, 0,
		2 * q.x * q.y + 2 * q.w * q.z, 1 - 2 * q.x * q.x - 2 * q.z * q.z, 2 * q.y * q.z - 2 * q.w * q.x, 0,
		2 * q.x * q.z - 2 * q.w * q.y, 2 * q.y * q.z + 2 * q.w * q.x, 1 - 2 * q.x * q.x - 2 * q.y * q.y, 0,
		0, 0, 0, 1 );
	mat = matQuaternion; 
}